package mybox.engine.linefollower.junit;

import java.awt.Point;
import java.awt.Polygon;
import java.util.ArrayList;

import at.innoc.util.obstacles.AAAPolygon;
import at.innoc.util.obstacles.DPoint;
import at.innoc.util.obstacles.exceptions.NotEndpointOfLineException;
import at.innoc.util.obstacles.exceptions.ParallelException;

import mybox.engine.linefollower.Carline;
import mybox.engine.linefollower.Course;
import mybox.engine.linefollower.DistDir;
import mybox.engine.linefollower.Polygon2QCAD;
import junit.framework.TestCase;

public class LineFollowerTC extends TestCase {

	public void test_PolygonFromSector() throws Exception {
		
		    int 	intFactor=100;
		    double 	schrittweite=10.0;		    
			ArrayList<Polygon> polygonList = Course.setupCourse(intFactor, schrittweite);
			Polygon2QCAD.show(polygonList);
			
			
			
		}
	
	public void test_CarPosList() throws Exception {		
	    Carline carline = new Carline(null,100,90,30);
	    Polygon2QCAD.show(carline.carposList);
	}
	
	public void testStreckeUndCarPos() throws Exception {
		
		int 	intFactor=100;
	    double 	schrittweite=10.0;		    
		ArrayList<Polygon> course = Course.setupCourse(intFactor, schrittweite);
		
		 Carline carline = new Carline(course,intFactor,80,18);
		 
		 ArrayList<Polygon> allPolys=Polygon2QCAD.merge(course, carline.carposList);
		 Polygon2QCAD.show(allPolys);
		
	}
	
	public void testMeasuringDistance2Course() throws Exception {
		
		int 	intFactor=100;
	    double 	schrittweite=10.0;
	    
		ArrayList<Polygon> course = Course.setupCourse(intFactor, schrittweite);
		Carline carline = new Carline(course,intFactor,90,30);
		
		int testPoint1X=carline.carposList.get(2).xpoints[0];
		int testPoint1Y=carline.carposList.get(2).ypoints[0];	
		Point pov = new Point(testPoint1X,testPoint1Y); 		
		DistDir distDir=calcMinDistance2Polygon(course, pov);
		System.out.println("distance="+distDir.distance+" direction="+distDir.direction);
		
		testPoint1X=carline.carposList.get(2).xpoints[1];
		testPoint1Y=carline.carposList.get(2).ypoints[1];	
		pov = new Point(testPoint1X,testPoint1Y); 		
		distDir=calcMinDistance2Polygon(course, pov);
		System.out.println("distance="+distDir.distance+" direction="+distDir.direction);
		
		testPoint1X=carline.carposList.get(3).xpoints[1];
		testPoint1Y=carline.carposList.get(3).ypoints[1];	
		pov = new Point(testPoint1X,testPoint1Y); 		
		distDir=calcMinDistance2Polygon(course, pov);
		System.out.println("distance="+distDir.distance+" direction="+distDir.direction);
		
		testPoint1X=carline.carposList.get(4).xpoints[1];
		testPoint1Y=carline.carposList.get(4).ypoints[1];	
		pov = new Point(testPoint1X,testPoint1Y); 		
		distDir=calcMinDistance2Polygon(course, pov);
		System.out.println("distance="+distDir.distance+" direction="+distDir.direction);
		
		testPoint1X=carline.carposList.get(5).xpoints[1];
		testPoint1Y=carline.carposList.get(5).ypoints[1];	
		pov = new Point(testPoint1X,testPoint1Y); 		
		distDir=calcMinDistance2Polygon(course, pov);
		System.out.println("distance="+distDir.distance+" direction="+distDir.direction);
		
		testPoint1X=carline.carposList.get(6).xpoints[1];
		testPoint1Y=carline.carposList.get(6).ypoints[1];	
		pov = new Point(testPoint1X,testPoint1Y); 		
		distDir=calcMinDistance2Polygon(course, pov);
		System.out.println("distance="+distDir.distance+" direction="+distDir.direction);
		
		
		
	}
	
	public DistDir calcMinDistance2Polygon(ArrayList<Polygon> polygonList, Point pov) {
		
		DistDir retVal=null;
		boolean ClctAndNoException=false;
		int tries=20;
		boolean povChanged=false;
		while (!ClctAndNoException && tries>0) {
			tries--;
			try {
					retVal=tryCalcminDistance2PolygonList(polygonList, pov);
				} catch (ParallelException ex) {
					pov.x=pov.x+3; pov.y=pov.y+3;
					povChanged=true;
				} catch (NotEndpointOfLineException e) {			
					pov.x=pov.x+3; pov.y=pov.y+3;
					povChanged=true;
				}
		}
		retVal.povChanged=povChanged;
		return retVal;
	}
	public DistDir tryCalcminDistance2PolygonList(ArrayList<Polygon> polygonList, Point pov) 
	throws NotEndpointOfLineException, ParallelException {
		
		
		AAAPolygon.pov=pov;			
		
		double[] sectorMinVal=null;
		
		ArrayList<DPoint> innerLine = AAAPolygon.closestPointsPolygon(polygonList, pov, AAAPolygon.DEBUGINFO_OFF);	
		int sectorAnz=360;	
		int [] sectorForbidden = AAAPolygon.rMinViolatingSectors(polygonList, pov, sectorAnz, 1.5);
		sectorMinVal = AAAPolygon.sectorMinDistance(innerLine,sectorForbidden, sectorAnz, pov,-1,0);
		
		double minAAAVal=100000;
		int minIndex=-1;
		for (int secIdx=0;secIdx<sectorMinVal.length;secIdx++) {	
			if (sectorMinVal[secIdx]<minAAAVal) {
				minAAAVal=sectorMinVal[secIdx];
				minIndex=secIdx;
			}
		}
		return new DistDir(minAAAVal,minIndex);
		
	}
	
}
